

#ifndef __BSTPHDNS_PLANNER_H__
#define __BSTPHDNS_PLANNER_H__

#include "cell_decomposer.h"
#include "ccpp_path_process.h"
#include "preprocess/map_process.h"

using namespace global_path_planner::preprocess;

#include <opencv2/opencv.hpp>

namespace global_path_planner {
namespace ccpp {

class BstphdnsPlanner{
public:
    explicit BstphdnsPlanner(std::shared_ptr<MapProcess>& map_process,
                            std::shared_ptr<CellDecomposer>& cell_decomp,
                            std::shared_ptr<Polygon2D>& poly2d);
    virtual ~BstphdnsPlanner();

    // 普通弓字形路径
    std::vector<std::vector<Point_2>>
    getBoustrophedonPath(Point_2& start, int& narrow_lane, cv::Mat& img,
                        float& sweep_width, float& sweep_interval,
                        bool& arc_boustrophedon, std::string& info,
                        std::vector<std::vector<cv::Point>> polys);

    // 圆弧弓字形路径
    std::vector<std::vector<Point_2>>
    getArcBoustrophedonPath(float &sweep_width, float &sweep_interval,
                            std::vector<std::vector<Point_2>> &ori_path);

    // 计算cells清扫路径
    std::vector<std::vector<Point_2>> getSweepPaths(int& narrow_lane,
                                                    float& sweep_width,
                                                    cv::Mat& map_img,
                                                    float& sweep_interval,
                                                    std::vector<Polygon_2>& bcd_cells);

private:

    // 计算清扫路径
    std::vector<std::vector<Point_2>> calcCellSweepPath(const Polygon_2& poly,
                                                        const FT offset,
                                                        const Direction_2& dir);

    // 规划清扫区域顺序
    std::deque<int> sortCellsSweep(std::vector<std::vector<Point_2>>& cells_sweeps,
                                    Point_2 start);

    // 重排相邻cell内的路径
    void sortAdjCellPaths(int &narrow_lane, float &sweep_width,
                        float &sweep_interval, std::deque<int> &cell_idx_path,
                        std::vector<std::vector<Point_2>> &cells_sweeps);

    // 优化弓字形
    std::vector<std::vector<Point_2>>
    getOptimizedBoustrophedonPaths(bool& arc_boustrophedon, float &sweep_width,
                                    float &sweep_interval, std::deque<int> &cell_idx_path,
                                    std::vector<std::vector<Point_2>> &cells_sweeps);

    // cell相邻圆弧路径
    std::vector<Point_2> getOneCellAdjArcPath(int lenth, float interval_radian,
                                            float adj_dx, float adj_dy,
                                            float arc_radius, float dx, float dy,
                                            float mid_x, float mid_y);

private:
    std::shared_ptr<CellDecomposer> __cell_Decomp_Ptr;
    std::shared_ptr<CCPPPathProcess> __ccpp_Proc_Ptr;
    std::shared_ptr<MapProcess> __map_Proc_Ptr;
    std::shared_ptr<Polygon2D> __polygon2d_Ptr;

};

}  // namespace ccpp
}  // namespace global_path_planner

#endif  // __BSTPHDNS_PLANNER_H__